This invention relates to the calibration of real-time multi-sensor tracking systems. More particularly, the invention relates to systems that use information from complimentary sensors, filtered by a simultaneously running filter component, to calibrate sensor bias and to iteratively tune the bias estimate.
Magnetic compasses are subject to several outside influences that produce errors in the compass reading. One of these error sources is magnetic variation, which is the difference between the direction of the field lines of the earth""s magnetic field and true north at any particular point on the earth, and which is a function of geographic position. Another source of error is magnetic deviation, which is the difference between magnetic north and the actual compass reading caused by local magnetic perturbations due to concentrations of metal or other magnetically active substances. Magnetic deviation is typically strong near large metal structures such as bridges, buildings with large steel components, and ships, or near structures that transport electricity such as power stations and electrical cables. Furthermore, in the case of ships or other large movable metal objects, the magnetic deviation may vary not only with direction, but may also vary over time in a particular direction.
Magnetic variation and static forms of magnetic deviation may be corrected in large part by modeling over the geographic region in question. Current approaches to bias determination rely either on a model of the bias, or on a number of external references and an involved calibration procedure. In the model-based approach, a mathematical model of how ambient magnetic fields modify compass output is used, and the calibration procedure uses a small number of external references to determine the parameters of the model. The heart of this method is the determination of a magnetic bias field that is fixed with respect to the compass, and thus moving with respect to the earth""s magnetic field. It involves superimposing and subtracting the mapped magnetic bias field to leave only the earth""s magnetic field in order to provide accurate compass output. Unfortunately, this approach has the disadvantage of being model-based, i.e. the bias must follow a general shape, which may not always be applicable to the measured bias. In the external reference approach, a large number of external references (e.g. landmarks at known headings) are used to build a distortion map. In this approach, known heading directions are used to calibrate the compass by comparing the compass reading with a known heading and subtracting the bias for that known heading to generate pure heading information. This approach requires many calibration points and a cumbersome, explicit calibration procedure. As a variation on this approach, prior art has also taught the use of a directional gyroscope to provide a second heading indicator to be compared with the compass heading to yield bias correction. This variation has the advantage of eliminating the need for predetermined, stored directions but is disadvantageous because the drift in the directional gyroscope causes it to become inaccurate over time. Existing systems utilizing this approach require an explicit calibration procedure, and drift out of calibration when the sensor distortion changes unless the calibration procedure is rerun. Thus, in the case of a ship, for example, although a mapped calibration procedure may be successful for a given ship configuration, any change in the structure of the ship or its contents may result in a need for re-calibration.
A method for dynamic autocalibration of a multi-sensor tracking system, having a system state xi within a state space, including the steps of: dividing the state space into a plurality of patches pj; providing a variable bias map including a plurality of bias entries {circumflex over (B)}j with each particular one of the plurality of bias entries {circumflex over (B)}j associated with a particular one of the plurality of patches pj; providing a vector zi of inputs from a plurality of sensors for a given time step i; determining the patch pj to which the vector zi from the plurality of sensors applies; combining the vector zi with the bias entry {circumflex over (B)}j corresponding to the patch pj to which the input zi from the plurality of sensors applies to provide a bias adjusted sensor input; providing a state estimator to receive the bias adjusted sensor input and to produce a system state estimation {circumflex over (x)}i corresponding to the time step i; using a combination of the bias adjusted sensor input and the system state estimation {circumflex over (x)}i to adjust the bias entry {circumflex over (B)}j of the variable bias map corresponding to the patch pj to which the input zi from the plurality of sensors applies; and repeating a portion of the steps to provide a continual system update. In the case where the bias map includes a constant bias offset, the method further includes, the steps of: obtaining a calibration bias pair including an externally specified calibration bias {circumflex over (B)}* and a corresponding system state calibration value x *; and applying the calibration bias pair to the variable bias map to eliminate the constant bias offset. The error minimization utilized in the present invention may be chosen as desired for a particular application, but is preferably performed by means of gradient descent utilizing a learning rate xcex3xe2x80x2 and the combination of the bias adjusted sensor input and the system state estimate {circumflex over (x)}i.
In addition to the use of a look-up table type bias map, the bias map may also be represented in the form of a parametric equation. An example of this type of bias map is provided through the use of Gaussian fuzzy sets, and is represented in a preferred embodiment including at least one gyroscope, at least one compass, and at least one tilt sensor incorporated as part of a head-mounted orientation tracker.